from distutils.log import info
import math
from re import T
from turtle import Turtle
from Source.CWJ.qyhf import check
from Method import collision_detection, get_Intersection_points, get_cos_value, blocks_select
import copy
from Avoidance import Avoid_left_right, Avoid_up_down, pipe_preprocess, Avoid_vertical, Avoid_move, Avoid_wall

class Node(object):
    def __init__(self, label, arr):
        self.label= label
        self.prev = None
        self.next = None
        self.arr = arr

class LinkedList(object):
    def __init__(self):
        self.head = None
        self.processed_pipes = []
        self.label_count = 0
    
    def is_Empty(self): #判断链表是否为空
        return self.head is None

    def create_LinkedList(self, pilist_arr): #输入管道列信息，创建双向链表
        for i in range(len(pilist_arr)):
            self.append_Node(pilist_arr[i])

    def add_Node(self, pipe_arr): #链表头部添加结点
        node = Node(self.label_count, pipe_arr)
        self.label_count += 1
        if self.is_Empty():
            self.head = node
        else:
            cursor = self.head
            cursor.prev = node
            node.next = cursor
            self.head = node

    def append_Node(self, pipe_arr): #链表尾部添加结点
        node = Node(self.label_count, pipe_arr)
        self.label_count += 1
        if self.is_Empty():
            self.head = node
        else:
            cursor = self.head
            while cursor.next is not None:
                cursor = cursor.next
            cursor.next = node
            node.prev = cursor   

    def insert_Node(self, label, pipe_arr): #在label位置处插入结点
        if self.is_Empty():
            print('链表为空，无法插入结点！')
        else:
            cursor = self.head
            while cursor is not None:
                if cursor.label == label:
                    if cursor == self.head:
                        self.add_Node(pipe_arr)
                    else:
                        node = Node(self.label_count, pipe_arr)
                        self.label_count += 1                    
                        node.next = cursor
                        node.prev = cursor.prev
                        cursor.prev.next = node
                        cursor.prev = node
                    break
                else:
                    cursor = cursor.next
    
    def delete_Node(self, label): #删除label位置处的结点
        if self.is_Empty():
            print('链表为空，无法删除结点！')
        else:
            cursor = self.head
            while cursor is not None:
                if cursor.label == label:
                    if cursor == self.head:
                        self.head = cursor.next
                        if cursor.next is not None:
                            cursor.next.prev = None
                    else:
                        cursor.prev.next = cursor.next
                        if cursor.next is not None:
                            cursor.next.prev = cursor.prev
                    break
                else:
                    cursor = cursor.next

    def output_Arr(self):
        array = []
        cursor = self.head
        while cursor is not None:
            array.append(cursor.arr)
            cursor = cursor.next          
        return array

def Avoid_obstacles(last_pi, pi, stru, block_list, temp, BlockBase, wall_list, PipeGroup_Type, ceilling, endpipe_flag):
    #information信息:[[新管道， 修正值], 状态, [上下避障的新管道, 上下避障后的修正值], 新的上一段管道, 超过天花板所处位置]
    if pi[0][2] > ceilling[1] or pi[0][2] < ceilling[0]:
        information = [[[pi], [0, 0, 0]], 'Fail', None, None, [None, pi[1]], None]

    if temp == 'Pipe': #若碰撞物是水管或风管
        print('障碍物为管道')
        information = Avoid_PipeOrDuct(last_pi, pi, stru, block_list, BlockBase, wall_list, PipeGroup_Type, ceilling, endpipe_flag)

    elif temp == 'Frame': #若碰撞物是横梁，上下避障
        print('障碍物为横梁')
        information = Avoid_Frame(last_pi, pi, stru, block_list, BlockBase, wall_list, PipeGroup_Type, ceilling, endpipe_flag)

    elif temp == 'Column': #若碰撞物是柱子，左右避障
        print('障碍物为柱子')
        information = Avoid_Column(last_pi, pi, stru, block_list, BlockBase, wall_list, PipeGroup_Type, ceilling, endpipe_flag)
        
    else:
        information = [[[pi], [0, 0, 0]], 'Fail', None, None, [None, None], None]
        
    if information[1] == 'Done':
        print('当前管道段避障成功')
        if_avoid = 1
        return information[0][0], information[0][1], information[3], information[4], information[5],if_avoid

    elif information[1] == 'Fail':
        print('当前管道段避障失败！返回原管道：', pi)
        if_avoid = 2
        return [pi], [0, 0, 0], None, information[4], information[5],if_avoid


def Avoid_Column(last_pi, pi, stru, block_list, BlockBase, wall_list, PipeGroup_Type, ceilling, endpipe_flag):
     # information信息:[[新管道， 修正值], 状态, [上下避障的新管道, 上下避障后的修正值], 新的上一段管道, 超过天花板所处位置] #
    information = [[[pi], [0, 0, 0]], 'Enter general avoidance', None, None, [None, None], None] 

    if information[0][0][0][0][0] == information[0][0][0][1][0] and information[0][0][0][0][1] == information[0][0][0][1][1]: #若管道为竖直
        return [[[pi], [0, 0, 0]], 'Done', None, None, [None, None], None] 

    cos_value = get_cos_value(pi, stru)
    intersection_points = get_Intersection_points(information[0][0][0], stru)
    if 0.966 < cos_value < 1 or -1 < cos_value < -0.966:
        information = pipe_preprocess(pi, stru, cos_value, PipeGroup_Type)
        if collision_detection(information[0][0][0], stru): 
            information[1] = 'Enter translational avoidance'
        else:
            return information
    elif cos_value == 1 or cos_value == -1:
        information[1] = 'Enter translational avoidance'
    elif intersection_points == []:
        information[1] = 'Enter translational avoidance'

    if information[1] == 'Enter general avoidance':
        information = Avoid_left_right(information[0][0][0], stru, block_list, BlockBase, wall_list, PipeGroup_Type, information[0][1], ceilling, Avoid_Column_flag = True)
    if information[1] == 'Enter translational avoidance':
        information = Avoid_move(last_pi, information[0][0][0], stru, block_list, PipeGroup_Type, BlockBase, wall_list, information[0][1], ceilling, endpipe_flag, information[2], information[4])
    return information

def Avoid_Frame(last_pi, pi, stru, block_list, BlockBase, wall_list, PipeGroup_Type, ceilling, endpipe_flag):
    # information信息:[[新管道， 修正值], 状态, [上下避障的新管道, 上下避障后的修正值], 新的上一段管道, 超过天花板所处位置] #
    information = [[[pi], [0, 0, 0]], 'Enter general avoidance', None, None, [None, None], None] 
    
    if information[0][0][0][0][0] == information[0][0][0][1][0] and information[0][0][0][0][1] == information[0][0][0][1][1]: #若管道为竖直
        return [[[pi], [0, 0, 0]], 'Done', None, None, [None, None], None] 
    
    cos_value = get_cos_value(pi, stru)
    intersection_points = get_Intersection_points(information[0][0][0], stru) #获取预处理后的管道和障碍物的碰撞坐标
    if 0.966 < cos_value < 1 or -1 < cos_value < -0.966:
        information = pipe_preprocess(pi, stru, cos_value, PipeGroup_Type)
        if collision_detection(information[0][0][0], stru): 
            information[1] = 'Enter translational avoidance'
        else:
            return information
    elif cos_value == 1 or cos_value == -1:
        information[1] = 'Enter translational avoidance'
    elif intersection_points == []:
        information[1] = 'Enter translational avoidance'

    if information[1] == 'Enter general avoidance':
        information = Avoid_up_down(information[0][0][0], stru, block_list, BlockBase, wall_list, PipeGroup_Type, information[0][1], ceilling)

    if information[1] == 'Enter translational avoidance':
        information = Avoid_move(last_pi, information[0][0][0], stru, block_list, PipeGroup_Type, BlockBase, wall_list, information[0][1], ceilling, endpipe_flag, information[2], information[4])
    return information

def Avoid_PipeOrDuct(last_pi, pi, stru, block_list, BlockBase, wall_list, PipeGroup_Type, ceilling, endpipe_flag): 
    # information信息:[[新管道， 修正值], 状态, [上下避障的新管道, 上下避障后的修正值], 新的上一段管道, 超过天花板所处位置] #
    information = [[[pi], [0, 0, 0]], 'Enter general avoidance', None, None, [None, None], None] 

    if information[0][0][0][0][0] == information[0][0][0][1][0] and information[0][0][0][0][1] == information[0][0][0][1][1]: #若管道为竖直
        return [[[pi], [0, 0, 0]], 'Done', None, None, [None, None], None] 
    
    cos_value = get_cos_value(pi, stru)
    intersection_points = get_Intersection_points(information[0][0][0], stru) #获取预处理后的管道和障碍物的碰撞坐标
    if intersection_points == []:
        information[1] = 'Enter translational avoidance'
    elif 0.966 < cos_value < 1 or -1 < cos_value < -0.966:
        information = pipe_preprocess(pi, stru, cos_value, PipeGroup_Type)
        if collision_detection(information[0][0][0], stru): 
            information[1] = 'Enter general avoidance'
        else:
            return information
    elif cos_value == 1 or cos_value == -1:
        information[1] = 'Enter general avoidance'
    

    if information[1] == 'Enter general avoidance':
        information = Avoid_left_right(information[0][0][0], stru, block_list, BlockBase, wall_list, PipeGroup_Type, information[0][1], ceilling, Avoid_Column_flag = False)
        if information[1] == 'Change to up-down':
            information = Avoid_up_down(information[0][0][0], stru, block_list, BlockBase, wall_list, PipeGroup_Type, information[0][1], ceilling)
    if information[1] == 'Enter translational avoidance':
        information = Avoid_move(last_pi, information[0][0][0], stru, block_list, PipeGroup_Type, BlockBase, wall_list, information[0][1], ceilling, endpipe_flag, information[2], information[4])
    return information            


def pilist_avoidance(linkedlist, block_list, blockBase, PipeGroup_Type, ceilling, pipeBase): #若同一列发生碰撞，则上下避开
    change_flag = False    
    unchangedlen_linkedlist = copy.deepcopy(linkedlist)
    cursor = linkedlist.head #将当前结点设为头结点
      
    while cursor is not None:
        other_linkedlist = copy.deepcopy(linkedlist) #除了被避障管道及其相邻管道的其他管道
        other_add_linkedlist = copy.deepcopy(linkedlist) #除了被避障管道、避障管道及其前一段管道的其他管道
        other_linkedlist.delete_Node(cursor.label) #删除当前结点的前一个结点
        if cursor.next is not None:
            other_linkedlist.delete_Node(cursor.next.label) #删除当前结点的后一个结点
        if cursor.prev is not None:
            other_linkedlist.delete_Node(cursor.prev.label) #删除当前结点的前一个节点
        other_add_linkedlist.delete_Node(cursor.label) #删除当前结点
        
        cursor2 = other_linkedlist.head #将当前结点设为头结点
        collision_node = None
        Currentpipe_fail_avoid = False #当前管道段避障失败的标志
        while cursor2 is not None:
            if [cursor.label, cursor2.label] in linkedlist.processed_pipes:
                cursor2 = cursor2.next
                continue
            is_collision = collision_detection(cursor.arr, cursor2.arr)
            if is_collision:
                collision_node = cursor2    
                other_add_linkedlist.delete_Node(cursor2.label) #删除当前结点
                if cursor2.prev is not None:
                    other_add_linkedlist.delete_Node(cursor2.prev.label) #删除当前结点的前一个结点
                break
            cursor2 = cursor2.next
        
        if collision_node == None:
            cursor = cursor.next
            continue
        else:
            linkedlist.processed_pipes.append([cursor.label, collision_node.label])
            linkedlist.processed_pipes.append([collision_node.label, cursor.label])
            pipe = collision_node.arr
            block = cursor.arr
            for i in range(len(block_list)): #添加障碍物，用于输入避障函数
                other_add_linkedlist.append_Node(block_list[i])
                
            other_add = other_add_linkedlist.output_Arr() #获取数组形式的管道

            # information信息:[[新管道， 修正值], 状态, [上下避障的新管道, 上下避障后的修正值], 新的上一段管道, 超过天花板所处位置] #
            information = Avoid_up_down(pipe, block, other_add, blockBase, [], PipeGroup_Type, [0, 0, 0], ceilling)
            if information[4][0] is None:
                new_pi = information[0][0]
                rectify = information[0][1]
            else:
                new_pi = information[2][0]
                rectify = information[2][1]
                cross_position = information[4][0]
                pipeBase.yellowBallCoordinates.append(cross_position)
                
            if new_pi != [pipe]: #若生成新的管道，将后面的管道衔接上
                change_flag = True #避障标志，True表示该管道已避障
                cursor3 = unchangedlen_linkedlist.head
                while cursor3 is not None:
                    if cursor3.label == collision_node.label:
                        break
                    else:
                        cursor3 = cursor3.next
                cursor3 = cursor3.next #从当前管道的下一段管道开始
                while cursor3 is not None:
                    cursor3.arr[0] = [cursor3.arr[0][i] + rectify[i] for i in range(3)]
                    cursor3.arr[1] = [cursor3.arr[1][i] + rectify[i] for i in range(3)]
                    cursor3 = cursor3.next        
            else:
                Currentpipe_fail_avoid = True
                
            if not Currentpipe_fail_avoid: #若当前管道段避障成功或标球，则更新:
                linkedlist.delete_Node(collision_node.label)
                if collision_node.next is not None:
                    insert_location = collision_node.next.label
                else:
                    insert_location = None
                for i in range(len(new_pi)):
                    if insert_location is not None:
                        linkedlist.insert_Node(insert_location, new_pi[i])
                    else:
                        linkedlist.append_Node(new_pi[i])

                cursor5 = linkedlist.head
                while cursor5 is not None:
                    if cursor5.label == insert_location:
                        break
                    else:
                        cursor5 = cursor5.next
                while cursor5 is not None:
                    cursor5.arr[0] = [cursor5.arr[0][i] + rectify[i] for i in range(3)]
                    cursor5.arr[1] = [cursor5.arr[1][i] + rectify[i] for i in range(3)]
                    cursor5 = cursor5.next
            else:
                pass
            break

    if cursor is not None:
         change_flag_recursion = pilist_avoidance(linkedlist, block_list, blockBase, PipeGroup_Type, ceilling, pipeBase)
         change_flag = change_flag or change_flag_recursion
         return change_flag
    else: #若遍历完管道列的最后一段，则返回
        return change_flag

def pilist_update(pilist, unchanged_len_pilist, j, information):
    new_pi = information[0]
    rectify = information[1]
    new_last_pi = information[2]
    for o in range(j + 1, len(unchanged_len_pilist)):
        unchanged_len_pilist[o][0] = [unchanged_len_pilist[o][0][i] + rectify[i] for i in range(3)]
        unchanged_len_pilist[o][1] = [unchanged_len_pilist[o][1][i] + rectify[i] for i in range(3)]
    if new_last_pi is not None:
        unchanged_len_pilist[j - 1] = new_last_pi
    unchanged_len_pilist[j] = new_pi
    pilist.pop(j)
    pilist.insert(j, new_pi)
    for o in range(j + 1, len(pilist)):
        pilist[o][0] = [pilist[o][0][i] + rectify[i] for i in range(3)]
        pilist[o][1] = [pilist[o][1][i] + rectify[i] for i in range(3)]
    if new_last_pi is not None: #若发生平移避障，则需更新前一段管道
        pilist[j - 1] = new_last_pi
    pilist[j] = new_pi

def Pi_bypass(original_pilist, block_wall_list, BlockBase, block_wall_list_recursion, PipeGroup_Type, ceilling):
    change_flag = False
    unchanged_len_pilist = copy.deepcopy(original_pilist)
    pilist = copy.deepcopy(original_pilist)
    if block_wall_list == []:
        print("该管道附近无障碍物")
        return change_flag, original_pilist, [[],[]],0

    wall_list = []
    block_list = []
    for i in range(len(block_wall_list_recursion)):
        type = check([block_wall_list_recursion[i]], BlockBase)
        if type == 'Wall':
            wall_list.append(block_wall_list_recursion[i])
        else:
            block_list.append(block_wall_list_recursion[i])
    
    wall_list_selected = blocks_select(unchanged_len_pilist, wall_list, 3000)

    if_avoid = 0  # 0 表示没有避障，1 表示 避障成功 2 表示避障失败
    cross_position_list = []
    fail_position_list = []
    for j in range(len(unchanged_len_pilist)): #当前列的每一段管道与障碍物避障
        if j != 0: #若当前管道不是第一段管道，将上一段管道传入避障函数中
            last_pi = unchanged_len_pilist[j - 1]
        else:
            last_pi = None

        if j == len(unchanged_len_pilist) - 1:
            endlist_flag = True
        else:
            endlist_flag = False
        
        for i in range(len(wall_list_selected)):
            is_collision = collision_detection(unchanged_len_pilist[j], wall_list_selected[i])
            if is_collision:
                
                cos_value = get_cos_value(unchanged_len_pilist[j], wall_list_selected[i])
                
                if 0.9659 <= cos_value <= 1 or -1 <= cos_value <= -0.9659:
                    information = Avoid_wall(last_pi, unchanged_len_pilist[j], wall_list_selected[i], block_list, wall_list_selected, ceilling)
                    fail_position_list.append(information[3])
                    if information[0] != unchanged_len_pilist[j]:
                        change_flag = True
                        pilist_update(pilist, unchanged_len_pilist, j, information) #将后面的管道衔接上
                    break
        Currentpipe_fail_avoid = False #当前管道段避障失败标志
        for i in range(len(block_list)):
            block_list_leave_changed = copy.deepcopy(block_list)
            is_collision = collision_detection(unchanged_len_pilist[j], block_list[i])
            type = check([block_list[i]], BlockBase)
            if is_collision:
                block_list_leave_changed[i] = None #将当前障碍物从列表中删除，用于传入避障函数
                block_list_leave_changed = list(filter(None, block_list_leave_changed))

                new_pi, rectify, new_last_pi, position, recursion_position,if_avoid = Avoid_obstacles(last_pi, unchanged_len_pilist[j], block_list[i], block_list_leave_changed, type, BlockBase, wall_list_selected, PipeGroup_Type, ceilling, endlist_flag)
                cross_position= position[0]
                fail_position = position[1]
                cross_position_list.append(cross_position)
                fail_position_list.append(fail_position)
                if recursion_position is not None:
                    for cross_point in recursion_position[0]:
                        cross_position_list.append(cross_point)
                    for fail_point in recursion_position[1]:
                        fail_position_list.append(fail_point)

                if new_pi != [unchanged_len_pilist[j]]: #若生成新的管道，将后面的管道衔接上
                    change_flag = True
                    for o in range(j + 1, len(unchanged_len_pilist)):
                        unchanged_len_pilist[o][0][0] = unchanged_len_pilist[o][0][0] + rectify[0]
                        unchanged_len_pilist[o][0][1] = unchanged_len_pilist[o][0][1] + rectify[1]
                        unchanged_len_pilist[o][0][2] = unchanged_len_pilist[o][0][2] + rectify[2]
                        unchanged_len_pilist[o][1][0] = unchanged_len_pilist[o][1][0] + rectify[0]
                        unchanged_len_pilist[o][1][1] = unchanged_len_pilist[o][1][1] + rectify[1]
                        unchanged_len_pilist[o][1][2] = unchanged_len_pilist[o][1][2] + rectify[2]
                    if new_last_pi is not None and j != 0: #若发生平移避障，则需更新前一段管道
                        unchanged_len_pilist[j - 1] = new_last_pi
                    if len(new_pi) == 1:
                        unchanged_len_pilist[j] = new_pi[0]
                    
                    if cross_position is not None:
                        Currentpipe_fail_avoid = False
                else:
                    Currentpipe_fail_avoid = True
            else:
                continue
            
            if Currentpipe_fail_avoid:
                break
            else:
                pilist.pop(j)
                for z in range(0, len(new_pi)):
                    pilist.insert(j + z, new_pi[z])
                for o in range(j + len(new_pi), len(pilist)):
                    pilist[o][0][0] = pilist[o][0][0] + rectify[0]
                    pilist[o][0][1] = pilist[o][0][1] + rectify[1]
                    pilist[o][0][2] = pilist[o][0][2] + rectify[2]
                    pilist[o][1][0] = pilist[o][1][0] + rectify[0]
                    pilist[o][1][1] = pilist[o][1][1] + rectify[1]
                    pilist[o][1][2] = pilist[o][1][2] + rectify[2]
                if new_last_pi is not None and j != 0: #若发生平移避障，则需更新前一段管道
                    pilist[j - 1] = new_last_pi
                break

    cross_position_list = list(filter(None, cross_position_list))
    fail_position_list = list(filter(None, fail_position_list))
    position_list = [cross_position_list, fail_position_list]
    return change_flag, pilist, position_list,if_avoid

def Set_slope(pilist, PipeGroup_Type, ceilling):
    change_flag = False
    if PipeGroup_Type == 'LinePipe': #若管道为点线水管
        for i in range(0, len(pilist)):
            change_flag = True
            len_pi = math.sqrt((pilist[i][1][0] - pilist[i][0][0]) ** 2 + (pilist[i][1][1] - pilist[i][0][1]) ** 2) #xy平面上的距离
            step = len_pi / 100  # z坐标应该下降的高度
            pilist[i][1][2] = pilist[i][1][2] - step
            if i != len(pilist) - 1: #将后面的管道衔接上
                for j in range(i + 1, len(pilist)):
                    pilist[j][0][2] = pilist[j][0][2] - step
                    pilist[j][1][2] = pilist[j][1][2] - step
    elif PipeGroup_Type == 'Pipe': #若管道为实体水管
        for i in range(0, len(pilist)):
            if (pilist[i][0][2] == pilist[i][1][2]):
                change_flag = True
                len_pi = math.sqrt((pilist[i][1][0] - pilist[i][0][0]) ** 2 + (pilist[i][1][1] - pilist[i][0][1]) ** 2) #xy平面上的距离
                step = len_pi / 100  # z坐标应该下降的高度
                pilist[i][1][2] = pilist[i][1][2] - step
                if i != len(pilist) - 1: #将后面的管道衔接上
                    for j in range(i + 1, len(pilist)):
                        pilist[j][0][2] = pilist[j][0][2] - step
                        pilist[j][1][2] = pilist[j][1][2] - step
    return change_flag, pilist